#include "sv2_gimbal_tracking.h"

namespace sv2
{
  // gimbal zoom scale
  double zoomScale = 1.0;

  nlohmann::json gimbal_ctl_msg = sms::def_msg("spirecv_msgs::GimbalControl");

  // 鼠标点击事件的回调函数
  void onMouse(int event, int x, int y, int flags, void *)
  {
    if (event == cv::EVENT_LBUTTONDOWN)
    {
      gimbal_ctl_msg["ctl_mode"] = 1;
      gimbal_ctl_msg["angle_euler_rate_cmd"] = {0, 0.02 * (y - 720 / 2), 0.02 * (x - 1280 / 2)};
    }
    if (event == cv::EVENT_RBUTTONDOWN)
    {
      gimbal_ctl_msg["ctl_mode"] = 2;
      gimbal_ctl_msg["angle_euler_cmd"] = {0, 0, 0};
    }
    if (event == cv::EVENT_MOUSEWHEEL)
    {
      int delta = cv::getMouseWheelDelta(flags);
      zoomScale += -delta * 0.1;
      gimbal_ctl_msg["zoom"] = std::max(1.0, std::min(10.0, zoomScale));
    }
  }

  void GimbalTrackingCppNode::run()
  {
    while (this->is_running())
    {

      // 主线程阻塞，等待队里中出现数据
      std::unique_lock<std::mutex> lock_vis(this->_full_vis_queue_mtx);
      this->_full_vis_cv.wait(lock_vis, [this]
                              { return !this->_full_vis_queue.empty(); });

      nlohmann::json image_vis_json = this->_full_vis_queue.front();
      this->_full_vis_queue.pop();

      cv::Mat image = sms::sms2cvimg(image_vis_json);
      nlohmann::json tgts_json = image_vis_json["spirecv_msgs::2DTargets"];

      cv::imshow(RGB_WINDOW, image);
      cv::waitKey(1);

      int img_h = tgts_json["height"], img_w = tgts_json["width"];

      if (tgts_json["targets"].size() == 0)
      {
        gimbal_ctl_msg["angle_euler_rate_cmd"] = {0, 0, 0};
        gimbal_ctl_msg["ctl_mode"] = 1;
      }
     
      for (int i = 0; i < (int)tgts_json["targets"].size(); i++)
      {
        if (tgts_json["targets"][i]["instance_id"] == this->tracked_id)
        {
          std::vector<float> b;
          for (int j = 0; j < tgts_json["targets"][0]["bbox"].size(); j++)
            b.push_back(tgts_json["targets"][0]["bbox"][j]);

          float cx = (b[0] + b[2] / 2.f) / img_w;
          float cy = (b[1] + b[3] / 2.f) / img_h;

          double s_target = b[2] * b[3];
          double st_target = img_w * img_h;

          gimbal_ctl_msg["move_mode"] = 1;
          gimbal_ctl_msg["angle_euler_rate_cmd"] = {0, this->trackDef * (cy - 0.5), this->trackDef * (cx - 0.5)};
          gimbal_ctl_msg["ctl_mode"] = 1;

          // Automatic Zoom, only for GX40
          if (s_target < st_target * 0.001)
          {
            zoomScale = zoomScale + 0.01;
            zoomScale = std::max(1.0, std::min(5.0, zoomScale));
            gimbal_ctl_msg["zoom"] = zoomScale;
          }
          else if (s_target > st_target * 0.08)
          {
            zoomScale = zoomScale - 0.01;
            zoomScale = std::max(1.0, std::min(5.0, zoomScale));
            gimbal_ctl_msg["zoom"] = zoomScale;
          }
        }
        else
        {
          gimbal_ctl_msg["angle_euler_rate_cmd"] = {0, 0, 0};
          gimbal_ctl_msg["ctl_mode"] = 1;
        }
      }

      _gimbal_ctl_pub.publish(gimbal_ctl_msg);
    }
  }
}
